#include <stdio.h> 
#include <stdlib.h> 

#include "usb_device.h"
#include "usbd_cdc.h"
//#include "usmart.h"
#include "malloc.h"
#include <ctype.h>						 /* Character functions                */
#include "math.h"
#include "../PUBLIC/Gamma_dll.h"

	//# 计算起始点位
	/*
	输入：
		  1、亮度调节使能，2、x调节使能，3、y调节使能；
		  4、R亮度采样值，5、G亮度采样值，6、B亮度采样值；
		  7、目标亮度值，8、目标x值，9、目标y值；
		  10、11、12、目标绑点R/G/B寄存器输入值；
		  13、目标灰阶，14、寄存器可调范围；
//----------------------------------------------------------------------------------
	输出：15，16，17、目标绑点R/G/B寄存器输出值；
		  18、超时提醒
	*/
	
	int Red_Ratio = 0xf;
	int Green_Ratio=0xf;
	int Blue_Ratio=0xf;
	//--------------系数----------------------------------------------------
	int Last_reg_red = 0;
	int Last_reg_green = 0;
    int Last_reg_blue = 0;
	//--------------多组计算------------------------------------------------
	int flexible_gamma_point_list[80]={0}; //存储灰阶
	int flexible_gamma_reg_red[80]={0};    //存储每阶R寄存器
	int flexible_gamma_reg_green[80]={0};  //存储每阶G寄存器
	int flexible_gamma_reg_blue[80]={0};   //存储每阶B寄存器
	int flexible_gamma_reg_count=0;
	//--------------单段计算------------------------------------------------
	int Past_Reg_stack[3][250];  //存储每跳步R/G/B寄存器值
	int Past_lum_stack[3][250];    //存储每跳步R/G/B分亮度值   
	double BCompare0[250];
	//--------------单点加速------------------------------------------------
	bool Register_jump_flag = false;  
	bool Register_lum_jump_flag = false;
	bool Gamma_adjust_timeout_flag = false;
	int Gamma_auto_adjust_count = 0;
	int Gamma_auto_adjust_count_temp = 0;
//	int Gamma_auto_adjust_count_set = 70;
	//--------------单色加速-------------------------------------------------
    int Y_adjust_count = 0;
	int X_adjust_count = 0;
	//-----------------------------------------------------------------------
	int Steph = 0, Stepm = 0, Stepl = 0;
	//-----------------------------------------------------------------------
	int Output_reg_red = 0; 
	int Output_reg_green = 0; 
	int Output_reg_blue = 0; 
	bool timeout_flag= false;	

    bool Flexible_Gamma_auto_Adujst(
	int lum_adjust_enable, int x_adjust_enable, int y_adjust_enable,
	double measure_lum, double measure_x, double measure_y,
	double target_lum, double target_x, double target_y,
	int input_reg_red, int input_reg_green, int input_reg_blue,
	int gary_target, int reg_range, int Gamma_auto_adjust_count_set
	//output
	)
	{
		int step_tmp = 0;
		double max_h, min_h, max_m, min_m;
		int adjust_times,shake_times,shake_max=5,shake_min=3;
		double target_reference;
		int reg_reference;
		int reg_approach = 0;
		Gamma_adjust_timeout_flag = false;
		Output_reg_red = input_reg_red;
		Output_reg_green = input_reg_green;
		Output_reg_blue = input_reg_blue;
		target_reference = target_lum + target_x + target_y;
		adjust_times = Gamma_auto_adjust_count_set;
		//自动单绑点调节次数
		if (reg_range > 0x7ff) //FFF~800
		{
			// adjust_times = 120;
			shake_max = 2;
			shake_min = 1;
		}
		else if (reg_range > 0x3ff) //7ff~400
		{
			// adjust_times = 90;
			shake_max = 3;
			shake_min = 2;
		}
		else
		{
			// adjust_times = 70;
			shake_max = 4;
			shake_min = 2;
		}

		if (Gamma_auto_adjust_count <= adjust_times)
		{
			if (Gamma_auto_adjust_count == 0)//firstly!clear buffer 
			{
				//根据可调range，自动定义步长，随灰阶绑点逐渐从Max减小至Min
				max_h = 50.13 * sin(0.0003539 * reg_range + 0.04827); 
				min_h = 30.06 * sin(0.0003796 * reg_range - 0.04927);
				max_m = 25.03 * sin(0.0004312 * reg_range - 0.24);
				min_m = 13.11 * sin(0.0002326 * reg_range - 0.08475);
				Steph = (int)(gary_target * max_h /255 + min_h);
				Stepm = (int)(gary_target * max_m /255 + min_m);
				Stepl = 1;

				if (gary_target == 255)//如果是每组的第一个绑点，清空List
				{			
					memset(flexible_gamma_point_list,0,80);
					memset(flexible_gamma_reg_red,0,80);
					memset(flexible_gamma_reg_green,0,80);
					memset(flexible_gamma_reg_blue,0,80);
					flexible_gamma_reg_count=0;
				}

				Register_jump_flag = false;
				Register_lum_jump_flag = false;
				//记录上个绑点调节最佳值
				Last_reg_red = Past_Reg_stack[0][Gamma_auto_adjust_count_temp];
				Last_reg_green = Past_Reg_stack[1][Gamma_auto_adjust_count_temp];
				Last_reg_blue = Past_Reg_stack[2][Gamma_auto_adjust_count_temp];

				//灰阶 list 有效数据从buffer[0]开始记录
				//flexible_gamma_point_list.Add(target_gray);
				//RGB寄存器 list 在数据清空前记录每阶绑点最佳值,有效数据从buffer[1]开始记录
				flexible_gamma_reg_red[flexible_gamma_reg_count]=Last_reg_red;
				flexible_gamma_reg_green[flexible_gamma_reg_count]=Last_reg_green;
				flexible_gamma_reg_blue[flexible_gamma_reg_count]=Last_reg_blue;
				flexible_gamma_reg_count++;
				
				memset(Past_Reg_stack,0,sizeof(Past_Reg_stack));
				memset(Past_lum_stack,0,sizeof(Past_lum_stack));
				memset(BCompare0,0,sizeof(BCompare0));
				Gamma_auto_adjust_count_temp = 0;//清“0”重新开始统计

				//加速跳转
				if ((gary_target != 255) && ((fabs(measure_lum - target_lum) > target_lum * 1 / 100)|| (fabs(measure_x - target_x) > target_x * 1 / 100)|| (fabs(measure_y - target_y) > target_x * 1 / 100)) && (((input_reg_red > Last_reg_red) || 
					(input_reg_red < Last_reg_red - (0x25 + 0x45 * gary_target / 255)) || ((input_reg_green > Last_reg_green) || (input_reg_green < Last_reg_green - (0x25 + 0x45 * gary_target / 255))) || ((input_reg_blue > Last_reg_blue) || (input_reg_blue < Last_reg_blue - (0x25 + 0x45 * gary_target / 255)))))) //R/G/B启动初始值干预.
				{
					Output_reg_red = Last_reg_red - Red_Ratio; //减去多少值后续由计算得出，此处暂定减去0xf
					Output_reg_green = Last_reg_green - Green_Ratio;
					Output_reg_blue = Last_reg_blue - Blue_Ratio;

					//output_reg_red = (int)(flexible_gamma_reg_red[1]) * pow((double)(gary_target / 255), (double)(1 / (3.266-0.0002604* reg_range))); //减去多少值后续由计算得出
					//output_reg_green = (int)(flexible_gamma_reg_green[1]) * pow((double)(gary_target / 255), (double)(1 / (3.266 - 0.0002604 * reg_range)));  //减去多少值后续由计算得出
					//output_reg_blue = (int)(flexible_gamma_reg_blue[1]) * pow((double)(gary_target / 255), (double)(1 / (3.266 - 0.0002604 * reg_range)));  //减去多少值后续由计算得出

					//output_reg_red= output_reg_red < Last_reg_red ? output_reg_red: Last_reg_red - Red_Ratio; //减去多少值后续由计算得出，此处暂定减去0xf
					//output_reg_green = output_reg_green < Last_reg_green ? output_reg_green : Last_reg_green - Green_Ratio;
					//output_reg_blue = output_reg_blue < Last_reg_blue ? output_reg_blue : Last_reg_blue - Blue_Ratio;

					Gamma_auto_adjust_count = Gamma_auto_adjust_count + 1;
					Gamma_auto_adjust_count_temp = Gamma_auto_adjust_count;//记录最佳值地址指针
					goto Jump_to_end;
				}
				else//debug
				{
					;
				}
			}
			Gamma_auto_adjust_count = Gamma_auto_adjust_count + 1;
			Gamma_auto_adjust_count_temp = Gamma_auto_adjust_count;//记录最佳值地址指针

			Past_Reg_stack[0][Gamma_auto_adjust_count] = input_reg_red;
			Past_Reg_stack[1][Gamma_auto_adjust_count] = input_reg_green;
			Past_Reg_stack[2][Gamma_auto_adjust_count] = input_reg_blue;
			Past_lum_stack[0][Gamma_auto_adjust_count] = measure_x;
			Past_lum_stack[1][Gamma_auto_adjust_count] = measure_lum;
			Past_lum_stack[2][Gamma_auto_adjust_count] = measure_y;
			BCompare0[Gamma_auto_adjust_count] = measure_x + measure_y + measure_lum;

			if (x_adjust_enable == 1)
			{
				if (fabs(measure_x - target_x) > target_x * 20 / 200)
				{
					step_tmp = target_lum < 0.5 ? Stepm : Steph;
				}
				else if (fabs(measure_x - target_x) > target_x * 2 / 200)
				{
					step_tmp = target_lum < 0.5 ? Stepl : Stepm; //if lower target lum
				}
				else
				{
					step_tmp = Stepl;
					X_adjust_count++;
				}
				if ((Gamma_auto_adjust_count > 5) && (Past_Reg_stack[0][Gamma_auto_adjust_count] == Past_Reg_stack[0][Gamma_auto_adjust_count - 2])) //滤掉第一次采样波动值
				{
					step_tmp = step_tmp / 2 < 1 ? Stepl : step_tmp / 2;
				}
				if (measure_x > target_x)
				{
					Output_reg_red = input_reg_red - step_tmp;
				}
				else if (measure_x < target_x)
				{
					Output_reg_red = input_reg_red + step_tmp;
				}
			}
			//-----------------------------------------------
			if (lum_adjust_enable == 1)
			{
				Y_adjust_count = 0;
				X_adjust_count = 0;
				if (fabs(measure_lum - target_lum) > target_lum * 20 / 200)
				{
					step_tmp = target_lum < 0.5 ? Stepm : Steph;
				}
				else if (fabs(measure_lum - target_lum) > target_lum * 2 / 200)
				{
					step_tmp = target_lum < 0.5 ? Stepl : Stepm;
				}
				else
				{
					step_tmp = Stepl;
				}
				//-------------------------step幅值干预---------------------------------
				if ((gary_target == 255) && (Gamma_auto_adjust_count >= 3) && (Register_lum_jump_flag == false)&&(measure_lum>0)) //滤掉第一次采样波动值
				{
					if ((fabs(measure_lum - target_lum) > target_lum * 3 / 10))//无需亮度过调
					{
						reg_approach = (int)(abs((int)(Past_Reg_stack[1][Gamma_auto_adjust_count] - Past_Reg_stack[1][Gamma_auto_adjust_count - 2])) * ((double)(target_lum / abs((Past_lum_stack[1][Gamma_auto_adjust_count] - Past_lum_stack[1][Gamma_auto_adjust_count - 2])))));
						step_tmp = (reg_approach > 0 && reg_approach < 0x3ff) ? reg_approach : step_tmp; //加速亮度逼近目标值
					}
					else
					{
						Register_lum_jump_flag = true;//停止加速
					}
				}
				if ((Gamma_auto_adjust_count > 5) && (Past_Reg_stack[1][Gamma_auto_adjust_count] == Past_Reg_stack[1][Gamma_auto_adjust_count - 2])) 
				{
					step_tmp = step_tmp / 2 < Stepl ? Stepl : step_tmp / 2;
				}
				if (measure_lum > target_lum)
				{
					Output_reg_green = input_reg_green - step_tmp;
				}
				else if (measure_lum < target_lum)
				{
					Output_reg_green = input_reg_green + step_tmp;
				}
			}
			//-----------------------------------------
			if (y_adjust_enable == 1)
			{
				X_adjust_count = 0;
				{
					if (fabs(measure_y - target_y) > target_y * 20 / 200)
					{
						step_tmp = target_lum < 0.5 ? Stepm : Steph;
					}
					else if (fabs(measure_y - target_y) > target_y * 2 / 200)
					{
						step_tmp = target_lum < 0.5 ? Stepl : Stepm;
					}
					else
					{
						step_tmp = Stepl;
						Y_adjust_count++;
					}
					if ((Gamma_auto_adjust_count>5)&&(Past_Reg_stack[2][Gamma_auto_adjust_count] == Past_Reg_stack[2][Gamma_auto_adjust_count - 2])) //滤掉第一次采样波动值
					{
						step_tmp = step_tmp / 2 <1? Stepl : step_tmp / 2;
					}
	 //               if (measure_y > target_y)
	 //                   output_reg_blue = input_reg_blue - step_tmp;
	//                else output_reg_blue = input_reg_blue + step_tmp;
					if (measure_y > target_y)
					{
						Output_reg_blue = input_reg_blue - step_tmp;
					}
					else if (measure_y < target_y)
					{
						Output_reg_blue = input_reg_blue + step_tmp;
					}
				}
			}

//----------------------------------------------------------------------------
			if ((fabs(BCompare0[Gamma_auto_adjust_count] - target_reference) < target_reference * (int)(shake_max - (double)((double)(gary_target/255)* shake_min)) / 100))//进入防抖范围
			{
				if ((step_tmp == Stepl) || (Stepl == Stepm))
				{
					if ((BCompare0[Gamma_auto_adjust_count - 2] > BCompare0[Gamma_auto_adjust_count - 1])&& (fabs(BCompare0[Gamma_auto_adjust_count-2] - target_reference) < target_reference * (int)((shake_max-1) - (double)((double)(gary_target / 255) * (shake_min-1))) / 100)&& (fabs(BCompare0[Gamma_auto_adjust_count-1] - target_reference) < target_reference * (int)((shake_max - 1) - (double)((double)(gary_target / 255) * (shake_min - 1))) / 100))
					{
						if (BCompare0[Gamma_auto_adjust_count - 1] > BCompare0[Gamma_auto_adjust_count])
						{
							if ((Y_adjust_count > 3) && (lum_adjust_enable != 1))
							// if (((Y_adjust_count > 3)&& (lum_adjust_enable != 1))||((x_adjust_enable!=1)&&((Math.Abs(measure_lum - target_lum) < target_lum * 2 / 100))))
							{
								if ((fabs(measure_y - target_y) < target_y * shake_max / 100) && (fabs(measure_lum - target_lum) < target_lum * shake_max / 100))
								{
									if (measure_y > target_y)
									{
										Output_reg_green = input_reg_green - Stepl;
									}
									else if (measure_y < target_y)
									{
										Output_reg_green = input_reg_green + Stepl;
									}
								}
								Y_adjust_count = 0;
							}
							/*
							if ((Math.Abs(measure_y - target_y) < target_y * 5 / 1000) && (Math.Abs(measure_x - target_x) < target_x * 5 / 1000) && (Math.Abs(measure_lum - target_lum) < target_lum * 1.5 / 100))
							{
								Gamma_auto_adjust_count = adjust_times;
							}
							if ((X_adjust_count > 3) && (y_adjust_enable != 1) && (lum_adjust_enable != 1))
							{
								if (Math.Abs(measure_x - target_x) < target_x * 7 / 100)
								{
									X_adjust_count = 0;
									Gamma_auto_adjust_count = adjust_times;
								}
							}
							*/
						}
						else
						{
							Output_reg_red = Past_Reg_stack[0][Gamma_auto_adjust_count];
							Output_reg_green = Past_Reg_stack[1][Gamma_auto_adjust_count];
							Output_reg_blue = Past_Reg_stack[2][Gamma_auto_adjust_count];
							Gamma_adjust_timeout_flag = true;
							//textBox1.Text = textBox1.Text + "[" + DateTime.Now.ToLongTimeString().ToString() + "]：" + "pass directly!!\r\n";
						}
					}
					else
					{
						;
					}
				}
				else if (step_tmp == Stepm)
				{
					Stepm = Stepm / 2 < Stepl ? Stepl : Stepm / 2;
/*
					if ((Y_adjust_count > 3) && (lum_adjust_enable != 1))
					{
						if ((Math.Abs(measure_y - target_y) < target_y * 5 / 100) && (Math.Abs(measure_lum - target_lum) < target_lum * 2 / 100))
						{
							if (measure_y > target_y)
							{
								output_reg_green = input_reg_green - Stepl;
							}
							else if (measure_y < target_y)
							{
								output_reg_green = input_reg_green + Stepl;
							}
						}
						Y_adjust_count = 0;
					}
					*/
				}
				else if (step_tmp == Steph)
				{
					Steph = Steph / 2< Stepm ? Stepm: Steph / 2;
				}
			}

			//----------------------------------若数值超限--------------------------------------                 
			/*if (((output_reg_red < 30) || (output_reg_red > index2_reg_red) || (output_reg_green < 30) || (output_reg_green > index2_reg_green) || (output_reg_blue < 30) || (output_reg_blue > index2_reg_blue)) && ((target_lum * 95 / 100 < measure_lum) && (target_lum * 105 / 100 > measure_lum)))
			{
				output_reg_red = Convert.ToInt16(index1_reg_red - Math.Abs(index2_reg_red - index1_reg_red) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target));
				output_reg_green = Convert.ToInt16(index1_reg_green - Math.Abs(index2_reg_green - index1_reg_green) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target));
				output_reg_blue = Convert.ToInt16(index1_reg_blue - Math.Abs(index2_reg_blue - index1_reg_blue) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target));
			//  textBox1.Text = "该绑点R/G/B插值迭代/r/n";
				Gamma_auto_adjust_count = 0;
				Gamma_adjust_timeout_flag = true;
			}*/
		}
		else
		{
			Y_adjust_count = 0;
			// textBox1.Text += "Timeout!\r\n";
			Gamma_auto_adjust_count = 0;
			Gamma_adjust_timeout_flag = true;
			/*
			if (output_reg_red > index1_reg_red)
			{
				output_reg_red = index1_reg_red - Math.Abs(index2_reg_red - index1_reg_red) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target);
				// textBox1.Text += "该绑点R差值迭代/r/n";
			}
			if (output_reg_green > index1_reg_green)
			{
				output_reg_green = index1_reg_green - Math.Abs(index2_reg_green - index1_reg_green) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target);
				//  textBox1.Text += "该绑点G差值迭代/r/n";
			}
			if (output_reg_blue > index1_reg_blue)
			{
				output_reg_blue = index1_reg_blue - Math.Abs(index2_reg_blue - index1_reg_blue) / (gary_last_index2 - gary_last_index1) * (gary_last_index1 - gary_target);
				//  textBox1.Text += "该绑点B差值迭代/r/n";
			}
			*/
		}
		Output_reg_red = Output_reg_red;
		Output_reg_green = Output_reg_green;
		Output_reg_blue = Output_reg_blue;

		if ((Gamma_auto_adjust_count>2)&&(lum_adjust_enable == 0) && (Register_jump_flag == false)) // only inter once time
		{
			Register_jump_flag = true; // 只跳转一次
			reg_reference = abs(Last_reg_green - Past_Reg_stack[1][Gamma_auto_adjust_count]); //得出差值

			if (fabs(measure_y - target_y) > target_y * 10 / 100)
			{
				if (Last_reg_green > Past_Reg_stack[1][Gamma_auto_adjust_count])
				{
					Output_reg_blue = Last_reg_blue - reg_reference;
				}
				else
				{
					Output_reg_blue = Last_reg_blue + reg_reference;
				}
			}
			if (fabs(measure_x - target_x) > target_x * 10 / 100)
			{
				if (Last_reg_green > Past_Reg_stack[1][Gamma_auto_adjust_count])
				{
					Output_reg_red = Last_reg_red - reg_reference;
				}
				else
				{
					Output_reg_red = Last_reg_red + reg_reference;
				}
			}
		}
		Jump_to_end:;
		return Gamma_adjust_timeout_flag;
	}
	
	
	
	void Lvxy_to_XYZ(double l_Lv, double l_x, double l_y)
	{
		L_X = l_Lv * l_x / l_y;
		L_Y = l_Lv;
		L_Z = (1 - l_x - l_y) * l_Lv / l_y;
	}